from Parameter import *
from pyqtgraph.Qt import QtGui, QtCore
import pyqtgraph as pg
import numpy as np
import time
import sys
from multiprocessing import Queue
from RecvProcess import *

import pygame
from pygame.locals import *
from OpenGL.GL import *
from OpenGL.GLU import *
from figure import *

roll_cube = np.zeros(CubeLen)
pitch_cube = np.zeros(CubeLen)
yaw_cube = np.zeros(CubeLen)
roll = 0
pitch = 0
yaw = 0


def Cube():
    glBegin(GL_LINES)
    for edge in edges:
        for vertex in edge:
            glVertex3fv(vertices[vertex])
    glEnd()


def InitPygame():
    global display
    pygame.init()
    display = (640, 480)
    pygame.display.set_mode(display, DOUBLEBUF | OPENGL)
    pygame.display.set_caption('IMU visual tool')


def InitGL():
    glClearColor((1.0 / 255 * 46), (1.0 / 255 * 45), (1.0 / 255 * 64), 1)
    glEnable(GL_DEPTH_TEST)
    glDepthFunc(GL_LEQUAL)
    glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST)

    gluPerspective(100, (display[0] / display[1]), 0.1, 50.0)
    glTranslatef(0.0, 0.0, -5)


def DrawText(textString):
    font = pygame.font.SysFont("Courier New", 25, True)
    textSurface = font.render(textString, True, (255, 255, 0), (46, 45, 64, 255))
    textData = pygame.image.tostring(textSurface, "RGBA", True)
    glDrawPixels(textSurface.get_width(), textSurface.get_height(), GL_RGBA, GL_UNSIGNED_BYTE, textData)


def DrawBoard():
    glBegin(GL_QUADS)
    x = 0

    for surface in surfaces:
        for vertex in surface:
            glColor3fv((colors[x]))
            glVertex3fv(vertices[vertex])
        x += 1
    glEnd()


def DrawGL(imu_Roll, imu_Pitch, imu_yaw):
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
    glLoadIdentity()
    gluPerspective(90, (display[0] / display[1]), 0.1, 50.0)
    glTranslatef(0.0, 0.0, -5)

    glRotatef(round(imu_Pitch, 1), 1, 0, 0)
    glRotatef(round(imu_Roll, 1), 0, 0, 1)
    glRotatef(round(imu_yaw, 1), 0, 1, 0)

    DrawText("Roll:{}°    yaw:{}°     Pitch:{}°".format(round(imu_Roll, 1), round(imu_yaw, 1), round(imu_Pitch, 1)))
    DrawBoard()
    pygame.display.flip()


if __name__ == '__main__':
    InitPygame()
    InitGL()

    rxQ = Queue()
    rx_thread = RecvProcess(rxQ)
    rx_thread.start()
    time.sleep(0.01)

    win = pg.GraphicsLayoutWidget(show=True)
    win.setWindowTitle('IMU waves display')
    p1 = win.addPlot(row=0, col=0)
    p1.setRange(xRange=[0, CubeLen], yRange=[-90, 90])
    curve1 = p1.plot(roll_cube, pen='g')

    p2 = win.addPlot(row=1, col=0)
    p2.setRange(xRange=[0, CubeLen], yRange=[-90, 90])
    curve2 = p2.plot(pitch_cube, pen='g')

    p3 = win.addPlot(row=2, col=0)
    p3.setRange(xRange=[0, CubeLen], yRange=[-90, 90])
    curve3 = p3.plot(yaw_cube, pen='g')


    def update1():
        global roll, pitch, yaw
        if not rxQ.empty():
            frame_data = rxQ.get_nowait()
            num, length = frame_data.shape
            # print(frame_data)
            for idx in range(num):
                tmp_roll = frame_data[idx, 21] * (2 ** 8) + frame_data[idx, 22]
                roll = trs15(tmp_roll) * 0.01
                tmp_pitch = frame_data[idx, 23] * (2 ** 8) + frame_data[idx, 24]
                pitch = trs15(tmp_pitch) * 0.01
                tmp_yaw = frame_data[idx, 25] * (2 ** 8) + frame_data[idx, 26]
                yaw = trs15(tmp_yaw) * 0.1

                roll_cube[:-1] = roll_cube[1:]  # shift data
                roll_cube[-1] = roll
                curve1.setData(roll_cube)

                pitch_cube[:-1] = pitch_cube[1:]  # shift data
                pitch_cube[-1] = pitch
                curve2.setData(pitch_cube)

                yaw_cube[:-1] = yaw_cube[1:]  # shift data
                yaw_cube[-1] = yaw
                curve3.setData(yaw_cube)

        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                pygame.quit()
                break

        DrawGL(roll, pitch, yaw)
        pygame.time.wait(1)


    timer = pg.QtCore.QTimer()
    timer.timeout.connect(update1)
    timer.start(2)

    if (sys.flags.interactive != 1) or not hasattr(QtCore, 'PYQT_VERSION'):
        QtGui.QApplication.instance().exec_()
